/*
 * @Description: 
 * @Author: Sang Hao
 * @Date: 2021-09-15 17:08:57
 * @LastEditTime: 2021-11-08 16:31:46
 * @LastEditors: Sang Hao
 */
#include "lidar_slam/models/cloud_filter/outlier_filter.hpp"

namespace lidar_slam {
	
bool OutlierFilter::SetFilterParam(int mean_k, float std_dev_mul_thresh) {
	outlier_filter_.setMeanK(mean_k);
	outlier_filter_.setStddevMulThresh(std_dev_mul_thresh);	/* 设置标准偏差阈值 */
	return true;
}

OutlierFilter::OutlierFilter(const YAML::Node& config_node) {
	int mean_k = config_node["mean_k"].as<int>();
	float std_dev_mul_thresh = config_node["std_dev_mul_thresh"].as<float>();
	SetFilterParam(mean_k, std_dev_mul_thresh);
}

OutlierFilter::OutlierFilter(int mean_k, float std_dev_mul_thresh) {
	SetFilterParam(mean_k, std_dev_mul_thresh);
}

bool OutlierFilter::Filter(const CloudData::CLOUD_PTR& input_cloud_ptr, 
		CloudData::CLOUD_PTR& filter_cloud_ptr) {
	outlier_filter_.setInputCloud(input_cloud_ptr);
	outlier_filter_.filter(*filter_cloud_ptr);
	
	return true;
}
}